﻿using Net.System;
using System;
using System.Collections.Generic;

namespace Net.FlowField
{
    // A* Pathfinder with path smoothing
    public static class AStarPathfinder
    {
        private static readonly Vector3[] Directions =
        {
            new Vector3(0, 0, 1),    // Up (Z+)
            new Vector3(1, 0, 0),    // Right (X+)
            new Vector3(0, 0, -1),   // Down (Z-)
            new Vector3(-1, 0, 0),   // Left (X-)
            new Vector3(1, 0, 1),    // Up-Right
            new Vector3(1, 0, -1),   // Down-Right
            new Vector3(-1, 0, 1),   // Up-Left
            new Vector3(-1, 0, -1)   // Down-Left
        };

        public static List<Vector3> FindPath(Simulator simulator, Vector3 start, Vector3 target)
        {
            var token = Environment.TickCount;

            // If target is unreachable (obstacle)
            int targetIndex = (int)(target.z * simulator.GridWidth + target.x);
            if (simulator.Grids[targetIndex].IsObstacleBool)
                return null;

            var openSet = new PriorityQueue<AStarNode>();
            var closedSet = new HashSet<Vector3>();
            var nodeGrid = simulator.NodeGrid;

            // Initialize start node
            var startNode = new AStarNode
            {
                Position = start,
                GCost = 0,
                HCost = CalculateHeuristic(start, target),
                Parent = null
            };
            nodeGrid[(int)(start.z * simulator.GridWidth + start.x)] = startNode;
            openSet.Enqueue(startNode);

            while (openSet.Count > 0)
            {
                var currentNode = openSet.Dequeue();
                closedSet.Add(currentNode.Position);

                // Path found
                if (currentNode.Position == target)
                {
                    var path = RetracePath(currentNode);
                    return SmoothPath(path, simulator);
                }

                foreach (var direction in Directions)
                {
                    var neighborPos = currentNode.Position + direction;

                    // Skip if invalid position or in closed set
                    if (!IsPositionValid(neighborPos,simulator.GridWidth, simulator.GridHeight) || closedSet.Contains(neighborPos))
                        continue;

                    int neighborIndex = (int)(neighborPos.z * simulator.GridWidth + neighborPos.x);

                    // Skip if obstacle
                    if (simulator.Grids[neighborIndex].IsObstacleBool)
                        continue;

                    // Calculate movement cost (14 for diagonal, 10 for straight)
                    int moveCost = (Math.Abs(direction.x) + Math.Abs(direction.z) == 2) ? 14 : 10;
                    int newGCost = currentNode.GCost + moveCost;

                    // Check if we found a better path to this neighbor
                    var neighborNode = nodeGrid[neighborIndex];
                    if (neighborNode == null || neighborNode.Token != token || newGCost < neighborNode.GCost)
                    {
                        if (neighborNode == null)
                        {
                            neighborNode = new AStarNode { Position = neighborPos };
                            nodeGrid[neighborIndex] = neighborNode;
                        }

                        neighborNode.GCost = newGCost;
                        neighborNode.HCost = CalculateHeuristic(neighborPos, target);
                        neighborNode.Parent = currentNode;
                        neighborNode.Position = neighborPos;
                        neighborNode.Token = token;

                        if (!openSet.Contains(neighborNode))
                            openSet.Enqueue(neighborNode);
                    }
                }
            }

            // No path found
            return null;
        }

        /// <summary>
        /// 路径平滑优化方法
        /// </summary>
        public static List<Vector3> SmoothPath(List<Vector3> originalPath, Simulator simulator)
        {
            if (originalPath == null || originalPath.Count <= 2)
                return originalPath;

            var smoothedPath = new List<Vector3>
            {
                originalPath[0]
            };

            for (int i = 1; i < originalPath.Count - 1; i++)
            {
                // 检查当前点与最后一个有效点之间是否有直线通路
                if (!HasLineOfSight(smoothedPath[smoothedPath.Count - 1], originalPath[i + 1], simulator))
                {
                    // 如果没有视线，添加当前点
                    smoothedPath.Add(originalPath[i]);
                }
            }

            // 添加终点
            smoothedPath.Add(originalPath[originalPath.Count - 1]);

            // 进一步优化：移除不必要的拐点
            return SimplifyPath(smoothedPath);
        }

        /// <summary>
        /// 检查两点之间是否有直线通路（无障碍物）
        /// </summary>
        private static bool HasLineOfSight(Vector3 start, Vector3 end, Simulator simulator)
        {
            // 使用Bresenham算法检查直线上的所有点
            int x0 = (int)start.x;
            int z0 = (int)start.z;
            int x1 = (int)end.x;
            int z1 = (int)end.z;

            int dx = Math.Abs(x1 - x0);
            int dz = Math.Abs(z1 - z0);
            int sx = x0 < x1 ? 1 : -1;
            int sz = z0 < z1 ? 1 : -1;
            int err = dx - dz;

            while (true)
            {
                // 检查当前点是否是障碍物
                if (x0 != (int)start.x || z0 != (int)start.z) // 跳过起点
                {
                    if (x0 != (int)end.x || z0 != (int)end.z) // 跳过终点
                    {
                        if (!IsPositionValid(new Vector3(x0, 0, z0), simulator.GridWidth, simulator.GridHeight))
                            return false;

                        int index = z0 * simulator.GridWidth + x0;
                        if (simulator.Grids[index].IsObstacleBool)
                            return false;
                    }
                }

                if (x0 == x1 && z0 == z1)
                    break;

                int e2 = 2 * err;
                if (e2 > -dz)
                {
                    err -= dz;
                    x0 += sx;
                }
                if (e2 < dx)
                {
                    err += dx;
                    z0 += sz;
                }
            }

            return true;
        }

        /// <summary>
        /// 简化路径，移除不必要的拐点
        /// </summary>
        private static List<Vector3> SimplifyPath(List<Vector3> path)
        {
            if (path.Count <= 2)
                return path;

            var simplified = new List<Vector3> { path[0] };
            Vector3 lastDirection = path[1] - path[0];

            for (int i = 1; i < path.Count - 1; i++)
            {
                Vector3 currentDirection = path[i + 1] - path[i];
                if (currentDirection != lastDirection)
                {
                    simplified.Add(path[i]);
                    lastDirection = currentDirection;
                }
            }

            simplified.Add(path[path.Count - 1]);
            return simplified;
        }

        private static List<Vector3> RetracePath(AStarNode endNode)
        {
            var path = new List<Vector3>();
            var currentNode = endNode;

            while (currentNode != null)
            {
                path.Add(currentNode.Position);
                currentNode = currentNode.Parent;
            }

            path.Reverse();
            return path;
        }

        private static int CalculateHeuristic(Vector3 a, Vector3 b)
        {
            // Manhattan distance for grid
            int dx = (int)Math.Abs(a.x - b.x);
            int dz = (int)Math.Abs(a.z - b.z);

            // D = 10, D2 = 14 (same as move costs)
            return 10 * (dx + dz) + (14 - 2 * 10) * Math.Min(dx, dz);
        }

        private static bool IsPositionValid(Vector3 pos, int gridWidth, int gridHeight)
        {
            return pos.x >= 0 && pos.x < gridWidth && pos.z >= 0 && pos.z < gridHeight;
        }
    }
}